#include <Eigen/Dense>
#include <string>
#include <string.h>
#include <sys/time.h>
#include <ctime>
#include <chrono>
#include <iomanip>
#include <stdio.h>
#include <iostream>

#include "global.h"
using namespace std;
using Eigen::Matrix3d;

Matrix3d R(string axis, double angle)// 计算x,y,z欧拉旋转矩阵
{
    angle = angle * 3.1415926 / 180.0; // sin函数参数为弧度制，需转换
    Matrix3d mat;
    if (axis == "x")
    {
        mat << 1, 0, 0,
            0, cos(angle), -sin(angle),
            0, sin(angle), cos(angle);
    }
    else if (axis == "y")
    {
        mat << cos(angle), 0, sin(angle),
            0, 1, 0,
            -sin(angle), 0, cos(angle);
    }
    else if (axis == "z")
    {
        mat << cos(angle), -sin(angle), 0,
            sin(angle), cos(angle), 0,
            0, 0, 1;
    }
    return mat;
}

#define time_print  //需要打印系统时间时保留

/*
    时间测量函数
    用法：
        #include <timetest.cpp>

        float tStart = timetest();
        //待测量代码段
        float tEnd = timetest();
        cout<<tEnd-tStart;
*/

float timetest()
{
    auto now = std::chrono::system_clock::now();
    // 通过不同精度获取相差的毫秒数
    uint64_t dis_millseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count() - std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count() * 1000;
    time_t tt = std::chrono::system_clock::to_time_t(now);
    auto time_tm = localtime(&tt);
    char strTime[25] = {0};
    float ttt = time_tm->tm_hour * 3600 + time_tm->tm_min * 60 + time_tm->tm_sec + 0.001*(float)dis_millseconds;
    #ifdef time_print
    sprintf(strTime, "%d-%02d-%02d %02d:%02d:%02d %03d", time_tm->tm_year + 1900, time_tm->tm_mon + 1, time_tm->tm_mday, time_tm->tm_hour, time_tm->tm_min, time_tm->tm_sec, (int)dis_millseconds);
    std::cout << strTime << std::endl;
    #else
    #endif

    return ttt;
}